

#include "scan_to_pointclod2_to_png.h"

#include <pcl/common/common.h>
#include <src/common/common.h>




ScanToPointCloud2Converter *scan_to_cloud_converter=NULL;

/**
 * 苏凯 2022-12-15
 *    roslaunch scantopnglocation   scan_to_scan_pose.launch   启动带有参数的
 */


Vector3d Mat2Euler(Matrix3d mat)
{

    return mat.eulerAngles(0, 1, 2);// x y z
}


VectorXd H2Euler(Matrix4d t)
{
    VectorXd pose = VectorXd(6);
    Matrix3d mt = t.block<3, 3>(0, 0);
    Vector3d p3 = t.block<3, 1>(0, 3).col(0);
    pose(0, 0) = p3.x();
    pose(1, 0) = p3.y();
    pose(2, 0) = p3.z();
    Vector3d eular = Mat2Euler(mt);
    pose(3, 0) = eular.x();
    pose(4, 0) = eular.y();
    pose(5, 0) = eular.z();
    return pose;
}


Matrix3d Euler2Mat(double rx, double ry, double rz)
{
    AngleAxisd rollAngle(AngleAxisd(rx, Vector3d::UnitX()));
    AngleAxisd pitchAngle(AngleAxisd(ry, Vector3d::UnitY()));
    AngleAxisd yawAngle(AngleAxisd(rz, Vector3d::UnitZ()));
    Matrix3d rotation_matrix;
    rotation_matrix = yawAngle * pitchAngle * rollAngle;
    return rotation_matrix;
}





Matrix4d Compose(Vector3d positon, Vector3d rotEular)
{
    Matrix3d rot =Euler2Mat(rotEular.x(), rotEular.y(), rotEular.z());
    // std::cout<<Mat2EulerAngle(rot);
    Matrix4d t;
    t.setIdentity();
    t.block<3, 3>(0, 0) = rot;
    t.block<3, 1>(0, 3) = positon;
    return t;
}


bool homogeneousInverse(Matrix4d &matrix4d,Matrix4d &matrix4dInverse) {

    if(matrix4d.rows()!=4 || matrix4d.cols()!=4){
        return false;
    }

    Matrix3d R;//旋转矩阵
    R = matrix4d.block<3, 3>(0, 0);
    //获取平移矩阵
    Eigen::Matrix<double, 3,1> t = matrix4d.block<3, 1>(0, 3);

    Matrix3d  Rt= R.transpose();
    Eigen::Matrix<double, 3,1> tinv = -Rt * t;

    matrix4dInverse.setIdentity();
    matrix4dInverse.block<3, 3>(0, 0) = Rt;
    matrix4dInverse.block<3, 1>(0, 3) = tinv;

    return true;

}


void ScanToPointCloud2Converter::filterCloud(
        const pcl::PointCloud<PointT>::Ptr &cloud,
        const pcl::PointCloud<PointT>::Ptr &cloud_filtered,
        const char *filter_name, double limitMin, double limitMax){
    pcl::PassThrough<PointT> passFilter;
    passFilter.setInputCloud(cloud);
    passFilter.setFilterFieldName(filter_name);
    passFilter.setFilterLimits(limitMin, limitMax);
    passFilter.setFilterLimitsNegative(false);
    passFilter.filter(*cloud_filtered);
}


cv::Mat ScanToPointCloud2Converter::eulerAnglesToRotationMatrix(cv::Vec3f &theta) {

    cv::Mat R_x = (cv::Mat_<double>(3, 3) <<
                                  1, 0, 0,
            0, cos(theta[0]), -sin(theta[0]),
            0, sin(theta[0]), cos(theta[0])
    );


    cv::Mat R_y = (cv::Mat_<double>(3, 3) <<
                                  cos(theta[1]), 0, sin(theta[1]),
            0, 1, 0,
            -sin(theta[1]), 0, cos(theta[1])
    );


    cv::Mat R_z = (cv::Mat_<double>(3, 3) <<
                                  cos(theta[2]), -sin(theta[2]), 0,
            sin(theta[2]), cos(theta[2]), 0,
            0, 0, 1
    );


    cv::Mat R = R_z * R_y * R_x;
    return R;
}


void ScanToPointCloud2Converter::init(){


}




bool ScanToPointCloud2Converter::isExDir(string &filepath){
                  std::vector<std::string>  filepathlist0 = split_data(filepath,"/");
                  if(filepathlist0.size()==0){
             
                      return false;
                  }
                std::string  filepathnew ="";

                  for (int j = 0; j <filepathlist0.size() ; ++j) {
                      filepathnew=filepathnew+"/"+filepathlist0[j];
                  }
                 std::cout<<"dir: "<<filepathnew<<std::endl;
                if(access(filepathnew.c_str(),0))
                {
                          std::string cmd = "mkdir -p " + filepathnew;
                            std::cout<<"cmd: "<<cmd<<std::endl;

                            system(cmd.c_str());
                 }
                 //----------------------------------------------------
                return true;
}


bool ScanToPointCloud2Converter::get_odom_angle(string pfamid, string cfamid,double &roll,double &pitch,double &yaw ){
    try{
        listener.lookupTransform(pfamid, cfamid, ros::Time(0),transformodom);
        tf::Matrix3x3(transformodom.getRotation()).getRPY(roll,pitch,yaw);
        std::cout<<"get_odom_angle 输出的欧拉角为 ：roll="<<roll<<",pitch="<<pitch<<",yaw="<<yaw<<std::endl;

    }catch (exception e){
        return false;
    }

    return true;
}

bool ScanToPointCloud2Converter::getrobotbaselink(string pfamid, string cfamid,double &roll,double &pitch,double &yaw ,geometry_msgs::Pose &pose,Matrix4d &Matrix4d_Transform){
    try{
        tf::StampedTransform transformbaselinkNew;
        listener.lookupTransform(pfamid, cfamid, ros::Time(0),transformbaselinkNew);
        pose.position.x=transformbaselinkNew.getOrigin().x();
        pose.position.y=transformbaselinkNew.getOrigin().y();
        pose.position.z=transformbaselinkNew.getOrigin().z();
        pose.orientation.x=transformbaselinkNew.getRotation().x();
        pose.orientation.y=transformbaselinkNew.getRotation().y();
        pose.orientation.z=transformbaselinkNew.getRotation().z();
        pose.orientation.w=transformbaselinkNew.getRotation().w();
        tf::Matrix3x3(transformbaselinkNew.getRotation()).getRPY(roll,pitch,yaw);

        Eigen::Vector3d laser2map_Transform_pos(pose.position.x, pose.position.y, pose.position.z);
        Eigen::Vector3d laser2map_Transform_rot(roll, pitch, yaw);
        Matrix4d_Transform=Compose(laser2map_Transform_pos, laser2map_Transform_rot);


    }catch (exception e){
        return false;
    }

    return true;
}


void ScanToPointCloud2Converter::initialPoseWithMatchTemplatestructs(){



    if ((matchTemplatestructs_.size() > 0 && rotatAngleStaticThreadnum>=(rotatAngleStatic_/angleStaticxishu))||startinitialpose==3) {
        timer3_.stop();
        double last_yaw= matchTemplatestructs_[matchTemplatestructs_.size()-1].yaw;
        sort(matchTemplatestructs_.begin(), matchTemplatestructs_.end(), [](MatchTemplatestruct a, MatchTemplatestruct b) {
            return a.maxVal > b.maxVal;
        });
        double x_Pixe_new = matchTemplatestructs_[0].x_Pixe_robotpose_template + matchTemplatestructs_[0].maxLoc.x;
        double y_Pixe_new = matchTemplatestructs_[0].y_Pixe_robotpose_template + matchTemplatestructs_[0].maxLoc.y;

        double rosX_new;
        double rosY_new;
        getglobalToRos(x_Pixe_new,y_Pixe_new,rosX_new,rosY_new);

        double yawnow= last_yaw-matchTemplatestructs_[0].yaw;

        geometry_msgs::PoseWithCovarianceStamped pose_msg;
        pose_msg.header.stamp = ros::Time::now();
        pose_msg.header.frame_id = "map";
        pose_msg.pose.pose.position.x = rosX_new;
        pose_msg.pose.pose.position.y = rosY_new;
        pose_msg.pose.covariance[0] = 0.25;
        pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
        pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;

        if(startinitialpose==3){
            geometry_msgs::Quaternion goal_quat2 = tf::createQuaternionMsgFromYaw(yawnow);
             pose_msg.pose.pose.orientation.z = goal_quat2.z;
             pose_msg.pose.pose.orientation.w = goal_quat2.w;
        }else{
            pose_msg.pose.pose.orientation.z = matchTemplatestructs_[0].qz;
            pose_msg.pose.pose.orientation.w = matchTemplatestructs_[0].qw;
        }

       if(matchTemplatestructs_[0].maxVal>fazhimin){

           initial_pose_pub.publish(pose_msg);

           geometry_msgs::PoseStamped poseStamped;
           poseStamped.header.stamp=ros::Time::now();
           poseStamped.header.frame_id = "map";
           poseStamped.pose.position.x=rosX_new;
           poseStamped.pose.position.y=rosY_new;
           poseStamped.pose.position.z=0.0;
           poseStamped.pose.orientation.z=pose_msg.pose.pose.orientation.z;
           poseStamped.pose.orientation.w=pose_msg.pose.pose.orientation.w;
           scanToPointToPng_poseStamped_pub.publish(poseStamped);

           string messages="initial_pose  rosX_new: "+to_string(rosX_new) +" ,rosY_new: "+to_string(rosY_new)+" ,qz: "+to_string(pose_msg.pose.pose.orientation.z)+" ,qw: "+to_string(pose_msg.pose.pose.orientation.w) ;
           contnav_debug(messages.c_str());

           string messages2="initial_pose  最大匹配分数maxVal: "+to_string(matchTemplatestructs_[0].maxVal) +" ,x_Pixe_new: "+to_string(x_Pixe_new)+" ,y_Pixe_new: "+to_string(y_Pixe_new)+" ,x_Pixe_robotpose_template: "+to_string(matchTemplatestructs_[0].x_Pixe_robotpose_template)+" ,y_Pixe_robotpose_template: "+to_string(matchTemplatestructs_[0].y_Pixe_robotpose_template)+" ,重定位 fazhimin: " + to_string(fazhimin);
           contnav_debug(messages2.c_str());

        } else{

           if(turnRepeatNum<=0){
               startinitialpose=-1;
               return;
           }


               if(turnRepeatInitialposeTtype=="static"){
                   rotatAngleStatic = rotatAngleStatic_;
                   turnRepeatNum=turnRepeatNum_;
                   startinitialpose=1;
                   rotatAngleStaticThreadnum=0;
                   while (rotatAngleStatic>0){
                       boost::thread ScanCallbackThread_thread(boost::bind(&ScanToPointCloud2Converter::ScanCallbackThread, this,rotatAngleStatic));
                       rotatAngleStatic=rotatAngleStatic-angleStaticxishu;
                   }
                   timer3_.start();
                   string messages2=" turnRepeatNum： "+to_string(turnRepeatNum)+" ,turnRepeatInitialposeTtype: "+turnRepeatInitialposeTtype;
                   contnav_debug(messages2.c_str());//日志
               }else if(turnRepeatInitialposeTtype=="dynamic"){
                   matchTemplatestructs_.clear();
                   rotatAngleStatic = rotatAngleStatic_;
                   turnRepeatNum=turnRepeatNum_;
                   rotatAngleStaticThreadnum=0;
                   startinitialpose=2;
                   boost::thread cmd_thread(boost::bind(&ScanToPointCloud2Converter::timerSendCmdFun, this));
                   timer3_.start();
               }else if(turnRepeatInitialposeTtype=="staticthread"){
                   matchTemplatestructs_.clear();
                   rotatAngleStatic = rotatAngleStatic_;
                   turnRepeatNum=turnRepeatNum_;
                   startinitialpose=1;
                   rotatAngleStaticThreadnum=0;
                   m_new_.unlock();
                   timer3_.start();
               }
               string messages2="再次重定位 turnRepeatNum： "+to_string(turnRepeatNum)+" ,turnRepeatInitialposeTtype: "+turnRepeatInitialposeTtype+" ,最大匹配分数maxVal: "+to_string(matchTemplatestructs_[0].maxVal)+" ,重定位 fazhimin: " + to_string(fazhimin);
               contnav_debug(messages2.c_str());//日志
               turnRepeatNum--;


       }

         startinitialpose=-1;


            cout<<"initialPoseWithMatchTemplatestructs"<<endl;
//        if(isshowImage){
//            Mat image2 = imread(image2path.c_str());
//            Rect rect1(matchTemplatestructs_[0].maxLoc.x, matchTemplatestructs_[0].maxLoc.y, matchTemplatestructs_[0].dst3w,
//                       matchTemplatestructs_[0].dst3h);
//            rectangle(image2, rect1, Scalar(0, 0, 255), 2);
//            imshow("dst", image2);
//            waitKey(3000);
//        }



    }


}
void ScanToPointCloud2Converter::timerinitialPoseWithMatchTemplatestructsFun(const ros::TimerEvent &time_event) {
    try {
        initialPoseWithMatchTemplatestructs();
    } catch (exception e) {
        cout << e.what() << endl;
    }
}


// const ros::TimerEvent &time_event
void ScanToPointCloud2Converter::timerSendCmdFun() {
    try {

        double roll, pitch, odom_angle;
        int odomfig=50;
        while (odomfig>1){
            bool fig=   get_odom_angle("/odom", "/base_link",roll,pitch,odom_angle );
            if(!fig){

                std::this_thread::sleep_for(std::chrono::milliseconds(500));
                odomfig--;
                continue;
            } else{
                break;
            }

        }
        if(odomfig<=1){
            return ;
        }

        if(odom_angle>M_PI){
            odom_angle += 2*M_PI;
        } else if (odom_angle<-M_PI){
            odom_angle -= 2*M_PI;
        }

        if (odom_angle<0&&odom_angle>=-M_PI){
            odom_angle += 2*M_PI;
        }
        double last_angle=odom_angle;
        double dynamic_turn_angle=0.0;
        double test_angle = dynamic_angle_*dynamic_angTora*dynamic_reverse;
        double error =test_angle  - dynamic_turn_angle;

        while (abs(error) > dynamic_tolerance ){
            if(ros::isShuttingDown()||startinitialpose==-1)
                return;

            geometry_msgs::Twist  twist;
            twist.angular.z=dynamic_speed;
            cmdpub.publish(twist);

            bool fig=   get_odom_angle("/odom", "/base_link",roll,pitch,odom_angle );
            if(!fig){
                continue;
            }
            if(odom_angle>M_PI){
                odom_angle += 2*M_PI;
            } else if (odom_angle<-M_PI){
                odom_angle -= 2*M_PI;
            }


            if (odom_angle<0&&odom_angle>=-M_PI){
              odom_angle += 2*M_PI;
                cout<<"2 odom_angle: "<<to_string(odom_angle)<<endl;
            }
             if(odom_angle<last_angle){
              odom_angle= odom_angle+2*M_PI;
            }


            double delta_angle = odom_angle-last_angle;
            dynamic_turn_angle+= delta_angle;
            error = test_angle - dynamic_turn_angle;
            last_angle = odom_angle;
            ScanCallbackDynamicThread(dynamic_turn_angle);


        }


        for (int j = 0; j < 20; ++j) {
            geometry_msgs::Twist  twist;
            twist.angular.z=0.0;
            cmdpub.publish(twist);
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        startinitialpose=3;

    } catch (exception e) {
        cout << e.what() << endl;
    }
}




ScanToPointCloud2Converter::ScanToPointCloud2Converter() : private_node_("~")
{
    srand(time(0));

    std::string home = getenv("HOME");
    std::string outPut =  "/home/sukai/workspace/scantopnglocation/src/scantopnglocation/output";
    pcdpath  = home + outPut;
    fileOutPut_=pcdpath;
    private_node_.param<string>("fileOutPut", fileOutPut_, pcdpath);
    pcdpath=fileOutPut_;

    string filelogpath =pcdpath+"/log";
    isExDir(filelogpath);
    imagesavepath=pcdpath+"/temp";//训练用的 图片保存，csv文件保存
    isExDir(imagesavepath);
    //日志
    string logFileName="scanTpngInitialPose.log";
    contnav::log::ContnavLogger::instance()->open(filelogpath+"/"+logFileName);
    contnav::log::ContnavLogger::instance()->level(m_level);
    contnav::log::ContnavLogger::instance()->max(m_max);


    private_node_.param<string>("mappath", image2path, "/home/sukai/slam/map/map_1.pgm");//地图路径
    contnav_info(image2path.c_str());

    private_node_.param<string>("laserFrameId", laserFrameId, "/laser");//雷达坐标系 laser  base_scan
    private_node_.param<int>("laser_index_min", laser_index_min_, 0);// 切割雷达数据，索引分割
    private_node_.param<int>("laser_index_max", laser_index_max_, 720);// 切割雷达数据，索引分割

    //取雷达方向的索引
    private_node_.param<int>("laser_middle_index", laser_middle_index, 180);// 雷达正中点  360
    private_node_.param<int>("laser_left_index", laser_left_index,90);// 取雷达左侧 180 的位置 180
    private_node_.param<int>("laser_right_index", laser_right_index, 270);// 取雷达右侧  270 的位置  540
    //局部图绘制小车半径
    private_node_.param<double>("robotradius", robotradius, 30);// 局部图绘制小车半径

    //雷达是否倒装
    private_node_.param<bool>("islaserInversion", islaserInversion, true);//雷达是否倒装

    private_node_.param<bool>("isshowImage", isshowImage, false);// 是否显示图片
    private_node_.param<bool>("showrobotposefig", showrobotposefig, false);// 是否在图片上图片显示车
    private_node_.param<bool>("isLocationinitialPosefig", isLocationinitialPosefig, false);// 融合定位得到坐标后是否执行 initialPose 来时时重定位,如果amcl端没有定位计算的坐标做融合，我们可以来时时 initialPose；
    private_node_.param<bool>("isshowImageWithRobotangle", isshowImageWithRobotangle, false);// 是否在局部图中显示车头位置对应的角度


    private_node_.param<int>("turnRepeatNum", turnRepeatNum_, 0);// 不满足打分的最低阀值时自动重新匹配的次数; 1 代表再执行一次重定位,2表示2次
    private_node_.param<string>("turnRepeatInitialposeTtype", turnRepeatInitialposeTtype, "static");// 选择用 static 静态重定位还是动态 dynamic 重定位做重定位失败后的再次自动重定位,配合 turnRepeatNum参数做


    //融合定位阀值
    private_node_.param<double>("fazhiLocation", fazhiLocation, 0.55);// 达到这个阀值后会从topic  /scanTpngPose中 发布出当前计算好的坐标出来

    //重定位阀值
    private_node_.param<double>("fazhimax", fazhimax, 0.62);// 最大匹配分数,达到这个值就会重新定位,不会把剩下没旋转完成的角度任务继续执行下去
    private_node_.param<double>("fazhimin", fazhimin, 0.4);// 低于这个阀值会触发再次自动重定位,会配合 turnRepeatNum_  与 turnRepeatInitialposeTtype 两参数完成再次重复重定位


    //动态重定位
    private_node_.param<int>("dynamic_reverse", dynamic_reverse, 1);// 控制正反转动 1 或 -1
    private_node_.param<double>("dynamic_angle", dynamic_angle_, 360);// 转一周的角度360度
    private_node_.param<double>("dynamic_tolerance", dynamic_tolerance, 0.2);// 小车转一周的允许误差
    private_node_.param<double>("dynamic_speed", dynamic_speed, 0.35);// 需要转一周的转的速度


    //静态重定位
    private_node_.param<double>("rotatAngleStatic", rotatAngleStatic_, 360);// 要旋转一周的角度
    private_node_.param<double>("angleStaticxishu", angleStaticxishu, 1);// 每次递减的角度值


    mapSub = node_handle_.subscribe<nav_msgs::OccupancyGrid>("/map", 100,  &ScanToPointCloud2Converter::callBack_map,this);
    cmdpub= node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1, true);
    initial_pose_pub = node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 10);

    timer3_ = node_handle_.createTimer(ros::Duration(2), &ScanToPointCloud2Converter::timerinitialPoseWithMatchTemplatestructsFun, this, false, false);

     cloud_msg =  PointCloudT::Ptr(new PointCloudT());
    scanToPointToPngServer = node_handle_.advertiseService<contnav_srvs::ScanToPointToPng::Request,contnav_srvs::ScanToPointToPng::Response>("/ScanToPointToPng", boost::bind(&ScanToPointCloud2Converter::callback_ScanToPointToPngrFun,this,_1,_2));
    ScanToPointToPngResult_publisher_ = node_handle_.advertise<std_msgs::String>("/ScanToPointToPngResult", 1, this);

    scanToPointToPng_poseStamped_pub = node_handle_.advertise<geometry_msgs::PoseStamped>("/scanTpngPose", 10);

    // 把图像发布出去
    cvBtidgepublisher = node_handle_.advertise<sensor_msgs::Image>("/dst2_map_image", 10);

    dst2_map_msgpublisher = node_handle_.advertise<contnav_msgs::ScanToPointToPng>("/dst2_map_msg", 10);
invalid_point_.x = 0.0;
invalid_point_.y = 0.0;
invalid_point_.z = 0.0;

   if(isshowImage)
    namedWindow("dst", WINDOW_NORMAL);// WINDOW_NORMAL 就是0,   WINDOW_AUTOSIZE 会自适应图像的尺寸。



}

ScanToPointCloud2Converter::~ScanToPointCloud2Converter()
{

   // viewer->close();
    ROS_INFO("Destroying ScanToPointCloud2Converter");
}



//service
bool ScanToPointCloud2Converter::callback_ScanToPointToPngrFun(contnav_srvs::ScanToPointToPng::Request &request ,contnav_srvs::ScanToPointToPng::Response &response) {

    //互锁
    m_scanToPointToPngrFun_.lock();

    bool fig =true;
    try {

        std_msgs::String data;

          if(request.request_type==ScanToPointToPngType::initialposeStatic ) {

             data.data=ScanToPointToPngType::initialposeStatic;
             ScanToPointToPngResult_publisher_.publish(data);

              string messages=ScanToPointToPngType::initialposeStatic;
              contnav_debug(messages.c_str());

             if( startinitialpose!=-1){

                 response.result=ServiceResultType::ResultTypeOK;
                 response.message="正在重定位计算,不能执行重定位";
                 contnav_error(response.message.c_str());
                 m_scanToPointToPngrFun_.unlock();
                 return true;
             }
             matchTemplatestructs_.clear();
             rotatAngleStatic = rotatAngleStatic_;
              turnRepeatNum=turnRepeatNum_;
              startinitialpose=1;
             rotatAngleStaticThreadnum=0;

             while (rotatAngleStatic>0 && startinitialpose!=-1){
                // ScanCallbackThread(rotatAngleStatic);
                 boost::thread ScanCallbackThread_thread(boost::bind(&ScanToPointCloud2Converter::ScanCallbackThread, this,rotatAngleStatic));
                 rotatAngleStatic=rotatAngleStatic-angleStaticxishu;
             }
             timer3_.start();

         }else  if(request.request_type==ScanToPointToPngType::initialposeDynamic ) {
             data.data=ScanToPointToPngType::initialposeDynamic;
             ScanToPointToPngResult_publisher_.publish(data);

              string messages=ScanToPointToPngType::initialposeDynamic;
              contnav_debug(messages.c_str());

              if(startinitialpose!=-1){

                 response.result=ServiceResultType::ResultTypeOK;
                  response.message="正在重定位计算,不能执行重定位";
                  contnav_error(response.message.c_str());
                 m_scanToPointToPngrFun_.unlock();
                 return true;
             }


              matchTemplatestructs_.clear();
              rotatAngleStatic = rotatAngleStatic_;
              turnRepeatNum=turnRepeatNum_;
              rotatAngleStaticThreadnum=0;

             startinitialpose=2;
                  boost::thread cmd_thread(boost::bind(&ScanToPointCloud2Converter::timerSendCmdFun, this));

             timer3_.start();

         }else  if(request.request_type==ScanToPointToPngType::initiallocation ) {
             data.data=ScanToPointToPngType::initiallocation;
             ScanToPointToPngResult_publisher_.publish(data);

              string messages=ScanToPointToPngType::initiallocation;
              contnav_debug(messages.c_str());

             if( startinitialpose!=-1){
                 response.result=ServiceResultType::ResultTypeOK;
                 response.message="正在重定位计算,不能执行重定位";

                 contnav_error(response.message.c_str());

                 m_scanToPointToPngrFun_.unlock();
                 return true;
             }
             matchTemplatestructs_.clear();
             rotatAngleStatic = rotatAngleStatic_;
             turnRepeatNum=turnRepeatNum_;


             rotatAngleStaticThreadnum=0;
             timer3_.stop();
             double ddd=0.0;//无实际意义
             for (int j = 0; j < 20; ++j) {
                 geometry_msgs::Twist  twist;
                 twist.angular.z=0.0;
                 cmdpub.publish(twist);
                 std::this_thread::sleep_for(std::chrono::milliseconds(50));
             }
             startinitialpose=6;
             std::this_thread::sleep_for(std::chrono::milliseconds(50));
             boost::thread ScanCallbackThread_thread(boost::bind(&ScanToPointCloud2Converter::ScanCallbacklocationThread, this,ddd));


         }else  if(request.request_type==ScanToPointToPngType::initialposeStop ) {
             data.data=ScanToPointToPngType::initialposeStop;
             ScanToPointToPngResult_publisher_.publish(data);

              string messages=ScanToPointToPngType::initialposeStop;
              contnav_debug(messages.c_str());

             startinitialpose=-1;
             matchTemplatestructs_.clear();
             rotatAngleStatic = rotatAngleStatic_;
              turnRepeatNum=turnRepeatNum_;
              rotatAngleStaticThreadnum=0;

             timer3_.stop();
             for (int j = 0; j < 20; ++j) {
                 geometry_msgs::Twist  twist;
                 twist.angular.z=0.0;
                 cmdpub.publish(twist);
                 std::this_thread::sleep_for(std::chrono::milliseconds(50));
             }
         }else if(request.request_type==ScanToPointToPngType::initialposeStaticthread ) {//静态重定位,多线程

              data.data=ScanToPointToPngType::initialposeStaticthread;
              ScanToPointToPngResult_publisher_.publish(data);

              string messages=ScanToPointToPngType::initialposeStaticthread;
              contnav_debug(messages.c_str());//日志

              if( startinitialpose!=-1){

                  response.result=ServiceResultType::ResultTypeOK;
                  response.message="正在重定位计算,不能执行重定位";
                  contnav_error(response.message.c_str());//日志
                  m_scanToPointToPngrFun_.unlock();
                  return true;
              }

              matchTemplatestructs_.clear();
              rotatAngleStatic = rotatAngleStatic_;
              turnRepeatNum=turnRepeatNum_;
              startinitialpose=1;
              rotatAngleStaticThreadnum=0;
              m_new_.unlock();
              scanCallbackThreadNew();
              timer3_.start();

          }else  if(request.request_type=="savefile" ) {//开始融合定位,保存当前图片与参数
              data.data=ScanToPointToPngType::initiallocation;
              ScanToPointToPngResult_publisher_.publish(data);

              string messages=ScanToPointToPngType::initiallocation;
              contnav_debug(messages.c_str());//日志

              if( startinitialpose!=-1){
                  response.result=ServiceResultType::ResultTypeOK;
                  response.message="正在重定位计算,不能执行重定位";

                  contnav_error(response.message.c_str());//日志

                  m_scanToPointToPngrFun_.unlock();
                  return true;
              }
              matchTemplatestructs_.clear();
              rotatAngleStatic = rotatAngleStatic_;
              turnRepeatNum=turnRepeatNum_;


              rotatAngleStaticThreadnum=0;
              timer3_.stop();
              double ddd=0.0;//无实际意义
              for (int j = 0; j < 20; ++j) {
                  geometry_msgs::Twist  twist;
                  twist.angular.z=0.0;
                  cmdpub.publish(twist);
                  std::this_thread::sleep_for(std::chrono::milliseconds(50));
              }
              startinitialpose=6;
              string  csvpath_name =imagesavepath+"/opencv.csv";
              out.open(csvpath_name, std::ios::out |std::ios::app);
              if (!out.is_open())
              {
                  std::cout << "file is not open " <<csvpath_name<< std::endl;
                  response.result=ServiceResultType::ResultTypeOK;
                  response.message="file is not open v"+csvpath_name;

                  contnav_error(response.message.c_str());//日志

                  m_scanToPointToPngrFun_.unlock();
                  return true;
              }


              issave=true;

              std::this_thread::sleep_for(std::chrono::milliseconds(50));
              boost::thread ScanCallbackThread_thread(boost::bind(&ScanToPointCloud2Converter::ScanCallbacklocationThread, this,ddd));


          }else  if(request.request_type=="stopSaveFile" ) {//停止savefile

              issave=false;
              data.data=ScanToPointToPngType::initialposeStop;
              ScanToPointToPngResult_publisher_.publish(data);

              string messages=ScanToPointToPngType::initialposeStop;
              contnav_debug(messages.c_str());//日志

              startinitialpose=-1;
              matchTemplatestructs_.clear();
              rotatAngleStatic = rotatAngleStatic_;
              turnRepeatNum=turnRepeatNum_;
              rotatAngleStaticThreadnum=0;

              timer3_.stop();
              for (int j = 0; j < 20; ++j) {
                  geometry_msgs::Twist  twist;
                  twist.angular.z=0.0;
                  cmdpub.publish(twist);
                  std::this_thread::sleep_for(std::chrono::milliseconds(50));
              }


              if (out.is_open())
              {
                  out.close();
              }



          }



    }catch (std::exception e){
        //互锁
        m_scanToPointToPngrFun_.unlock();
        cout<<  e.what()<<endl;
        string messages="callback_ScanToPointToPngrFun()->exception ";
        ROS_ERROR("callback_ScanToPointToPngrFun()->exception ");
        contnav_error(messages.c_str());//日志

    }
    //延时
    // ros::Rate r((0.2));
    //  r.sleep();

    if(fig){
        response.result=ServiceResultType::ResultTypeOK;
    }else{
        response.result=ServiceResultType::ResultTypeERROR;
    }
    //互锁
    m_scanToPointToPngrFun_.unlock();
    return true;
}



//订阅地图
void ScanToPointCloud2Converter::callBack_map(nav_msgs::OccupancyGrid msg){

    origin_x = msg.info.origin.position.x;
    origin_y = msg.info.origin.position.y;
    origin_z = msg.info.origin.position.z;
    origin_qx = msg.info.origin.orientation.x;
    origin_qy = msg.info.origin.orientation.y;
    origin_qz = msg.info.origin.orientation.z;
    origin_qw = msg.info.origin.orientation.w;
    resolution_f = msg.info.resolution;
    width = msg.info.width;
    height = msg.info.height;





}



void ScanToPointCloud2Converter::rosToGlobal_one(double rosX,double rosY,double fx,double fy,double scene_x,double scene_y,double &x_Pixe,double &y_Pixe){

    x_Pixe = rosX / fx + scene_x;

    y_Pixe =  scene_y-rosY / fy;


}
void ScanToPointCloud2Converter::getrosToGlobal(double rosX,double rosY,double &x_Pixe,double &y_Pixe){
    double scene_x=0-(origin_x/resolution_f);
    double scene_y=height+( origin_y/resolution_f);
    rosToGlobal_one(rosX,rosY,resolution_f,resolution_f,scene_x,scene_y,x_Pixe,y_Pixe);
}

void ScanToPointCloud2Converter::globalToRos_one(double x_Pixe,double y_Pixe,double fx,double fy,double scene_x,double scene_y,double &rosX,double &rosY){
    rosX = (x_Pixe-scene_x)*fx;
    rosY = (scene_y - y_Pixe)*fy;

}
void ScanToPointCloud2Converter::getglobalToRos(double x_Pixe_new,double y_Pixe_new,double &rosX_new,double &rosY_new){
    double scene_x=0-(origin_x/resolution_f);
    double scene_y=height+( origin_y/resolution_f);
    globalToRos_one(x_Pixe_new,y_Pixe_new,resolution_f,resolution_f,scene_x,scene_y,rosX_new,rosY_new);

}

int ScanToPointCloud2Converter::scale_to_255(int pixel_value, int min, int max) {
    return int((((pixel_value - min) / float(max - min)) * 255));
}


void ScanToPointCloud2Converter::use_pcdTopicture_main(const double &dynamic_turn_angle) {

    pcl::PointXYZ min, max;

    pcl::getMinMax3D(*cloud_msg, min, max);


    vector<int>indices;
    cout << "2cloud_msg->points.size(): "  << cloud_msg->points.size()<<endl;
    for (int i = 0; i < cloud_msg->points.size(); i++) {
            indices.push_back(i);

    };

    vector<int>x_img;
    vector<int>y_img;

     int x_max =0;
     int y_max =0;
    int x_min =10000;
    int y_min =10000;
    double scene_x=0-(origin_x/resolution_f);
    double scene_y=height+( origin_y/resolution_f);


    cout << "indices.size(): "  << indices.size()<<endl;
    for (int i = 0; i < indices.size(); i++) {
        if((abs(cloud_msg->points[i].x)+abs(cloud_msg->points[i].y))<0.000001)
            continue;


        double  rosX=cloud_msg->points[i].x;
        double  rosY=cloud_msg->points[i].y;

        double x_Pixe_new;
        double y_Pixe_new;
        rosToGlobal_one(rosX,rosY,resolution_f,resolution_f,scene_x,scene_y,x_Pixe_new,y_Pixe_new);
        x_img.push_back(x_Pixe_new<0?0:x_Pixe_new);
        y_img.push_back(y_Pixe_new<0?0:y_Pixe_new);

        if(x_Pixe_new>x_max)
            x_max=x_Pixe_new;
        if(y_Pixe_new>y_max)
            y_max=y_Pixe_new;

        if(x_Pixe_new<x_min)
            x_min=x_Pixe_new<0?0:x_Pixe_new;
        if(y_Pixe_new<y_min)
            y_min=y_Pixe_new<0?0:y_Pixe_new;

    }


    double roll,pitch,yaw;
    Matrix4d matrix4d_Transform;

    geometry_msgs::Pose pose_base_link;
    bool fig= false;
    for (int j = 0; j < 10; ++j) {
        if(startinitialpose==2){
            fig =  getrobotbaselink("/base_link", laserFrameId,roll,pitch,yaw ,pose_base_link,matrix4d_Transform);
        }else{
            fig =  getrobotbaselink("/map", "/base_link",roll,pitch,yaw ,pose_base_link,matrix4d_Transform);
        }



        if(!fig)
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    if(!fig){
        cout << "未获取到robotpose的位置！"  << endl;

        m_.unlock();
        return;
    }


    double x_Pixe_robotpose;
    double y_Pixe_robotpose;

    if(startinitialpose==2){

          getrosToGlobal(0.0,0.0,x_Pixe_robotpose,y_Pixe_robotpose);
    }else{
          getrosToGlobal(pose_base_link.position.x,pose_base_link.position.y,x_Pixe_robotpose,y_Pixe_robotpose);

    }





   if(showrobotposefig&&isshowImage){

       x_img.push_back(x_Pixe_robotpose);
       y_img.push_back(y_Pixe_robotpose);


       if(x_Pixe_robotpose>x_max)
           x_max=x_Pixe_robotpose;
       if(y_Pixe_robotpose>y_max)
           y_max=y_Pixe_robotpose;

       if(x_Pixe_robotpose<x_min)
           x_min=x_Pixe_robotpose;
       if(y_Pixe_robotpose<y_min)
           y_min=y_Pixe_robotpose;
   }






    y_min=y_min>10?y_min-10:y_min;
    x_min=x_min>10?x_min-10:x_min;


     cv::Mat im = cv::Mat::zeros(y_max-y_min+10, x_max-x_min+10, CV_8U);

    for (int i = 0; i < x_img.size(); i++) {//233

        im.at<uchar>((y_img[i]-y_min), (x_img[i]-x_min)) = 255;

    }


    double  x_Pixe_robotpose_template=x_Pixe_robotpose-x_min;
    double  y_Pixe_robotpose_template=y_Pixe_robotpose-y_min;
    if(showrobotposefig&&isshowImage){
        cout << "x_Pixe_robotpose_template: "  <<x_Pixe_robotpose_template<< endl;
        cout << "y_Pixe_robotpose_template: "  <<y_Pixe_robotpose_template<< endl;
        im.at<uchar>(y_Pixe_robotpose_template, x_Pixe_robotpose_template) = 150;
    }

    if(isshowImage)
       cv::imshow("img_result", im);



    Mat dst;

    Mat kernel = getStructuringElement(MORPH_RECT,Size(10,10));
    dilate(im,dst,kernel);
    if(isshowImage)
        cv::imshow("img_result2", dst);

    Mat dst1;
    Mat kernel2 = getStructuringElement(MORPH_RECT,Size(8,8));

    erode(dst,dst1,kernel2);


    bool laser_middlefig=false;
    bool laser_leftfig=false;
    bool laser_rightfig=false;
    double   leftangle;
    double   rightangle;
    if(isshowImageWithRobotangle && isshowImage){

        cv::Mat dsttest;

        dsttest=  dst1;
        double laser_middle_x=cloud_msg->points[laser_middle_index].x;
        double laser_middle_y=cloud_msg->points[laser_middle_index].y;
        double laser_Pixe_middle_x;
        double laser_Pixe_middle_y;

        getrosToGlobal(laser_middle_x,laser_middle_y,laser_Pixe_middle_x,laser_Pixe_middle_y);
        laser_Pixe_middle_x=laser_Pixe_middle_x-x_min;
        laser_Pixe_middle_y=laser_Pixe_middle_y-y_min;

        if((abs(cloud_msg->points[laser_middle_index].x)+abs(cloud_msg->points[laser_middle_index].y))>0){
            laser_middlefig=true;


        }

        Point circlecenter;
        circlecenter.x=x_Pixe_robotpose_template;
        circlecenter.y=y_Pixe_robotpose_template;
        circle(dsttest,circlecenter,robotradius,Scalar(150,0,0),0.5);


        double laser_left_x=cloud_msg->points[laser_left_index].x;
        double laser_left_y=cloud_msg->points[laser_left_index].y;
        double laser_Pixe_left_x;
        double laser_Pixe_left_y;
        getrosToGlobal(laser_left_x,laser_left_y,laser_Pixe_left_x,laser_Pixe_left_y);
        laser_Pixe_left_x=laser_Pixe_left_x-x_min;
        laser_Pixe_left_y=laser_Pixe_left_y-y_min;

        if((abs(cloud_msg->points[laser_left_index].x)+abs(cloud_msg->points[laser_left_index].y))>0){
            laser_leftfig=true;

        }


        double laser_right_x=cloud_msg->points[laser_right_index].x;
        double laser_right_y=cloud_msg->points[laser_right_index].y;
        double laser_Pixe_right_x;
        double laser_Pixe_right_y;
        getrosToGlobal(laser_right_x,laser_right_y,laser_Pixe_right_x,laser_Pixe_right_y);
        laser_Pixe_right_x=laser_Pixe_right_x-x_min;
        laser_Pixe_right_y=laser_Pixe_right_y-y_min;

        if((abs(cloud_msg->points[laser_right_index].x)+abs(cloud_msg->points[laser_right_index].y))>0){

            laser_rightfig=true;

        }

        if(laser_leftfig){


            double PI = 3.141592653;
            double vec1_x =laser_Pixe_left_x-x_Pixe_robotpose_template;
            double vec1_y =laser_Pixe_left_y-y_Pixe_robotpose_template;
            double vec2_x =(x_Pixe_robotpose_template+robotradius)-x_Pixe_robotpose_template;
            double vec2_y =y_Pixe_robotpose_template-y_Pixe_robotpose_template;
            double t = (vec1_x * vec2_x + vec1_y * vec2_y) / (sqrt(pow(vec1_x, 2) + pow(vec1_y, 2)) * sqrt(pow(vec2_x, 2) + pow(vec2_y, 2)));

            leftangle = acos(t) ;
            if(y_Pixe_robotpose_template<laser_Pixe_left_y)
                leftangle=-leftangle;

            if (leftangle<0&&leftangle>=-M_PI){
                leftangle += 2*M_PI;
            }

        }


        if(laser_rightfig){

            double PI = 3.141592653;
            double vec1_x =laser_Pixe_right_x-x_Pixe_robotpose_template;
            double vec1_y =laser_Pixe_right_y-y_Pixe_robotpose_template;
            double vec2_x =(x_Pixe_robotpose_template+robotradius)-x_Pixe_robotpose_template;
            double vec2_y =y_Pixe_robotpose_template-y_Pixe_robotpose_template;
            double t = (vec1_x * vec2_x + vec1_y * vec2_y) / (sqrt(pow(vec1_x, 2) + pow(vec1_y, 2)) * sqrt(pow(vec2_x, 2) + pow(vec2_y, 2)));
            rightangle = acos(t) ;

            if(y_Pixe_robotpose_template<laser_Pixe_right_y)
                rightangle=-rightangle;

            if (rightangle<0&&rightangle>=-M_PI){
                rightangle += 2*M_PI;
            }

        }


        if(!laser_middlefig){
            if(laser_leftfig){
                if(islaserInversion){
                    middleangle= leftangle-1.57;
                }else{
                    middleangle= leftangle+1.57;

                }

                if (middleangle<0&&middleangle>=-M_PI){
                    middleangle += 2*M_PI;
                }
            }
            else if(laser_rightfig){

                if(islaserInversion){
                    middleangle= rightangle+1.57;
                }else{
                    middleangle= rightangle-1.57;
                }


                if (middleangle>=2*M_PI){
                    middleangle -= 2*M_PI;
                }

            }else{

            }

        }else{


            double PI = 3.141592653;
            double vec1_x =laser_Pixe_middle_x-x_Pixe_robotpose_template;
            double vec1_y =laser_Pixe_middle_y-y_Pixe_robotpose_template;
            double vec2_x =(x_Pixe_robotpose_template+robotradius)-x_Pixe_robotpose_template;
            double vec2_y =y_Pixe_robotpose_template-y_Pixe_robotpose_template;
            double t = (vec1_x * vec2_x + vec1_y * vec2_y) / (sqrt(pow(vec1_x, 2) + pow(vec1_y, 2)) * sqrt(pow(vec2_x, 2) + pow(vec2_y, 2)));

            middleangle = acos(t) ;

            if(y_Pixe_robotpose_template<laser_Pixe_middle_y)
                middleangle=-middleangle;
            if (middleangle<0&&middleangle>=-M_PI){ //换算到0-6.28之间
                middleangle += 2*M_PI;
            }

        }
        if(laser_middlefig||!laser_leftfig||!laser_rightfig){
            line(dsttest, Size(x_Pixe_robotpose_template ,y_Pixe_robotpose_template) , Size(x_Pixe_robotpose_template + robotradius * cos(-middleangle), y_Pixe_robotpose_template + robotradius * sin(-middleangle)), Scalar(64, 255, 0), 1, LINE_AA);

        }else{

            middleangle=yaw;
            line(dsttest, Size(x_Pixe_robotpose_template ,y_Pixe_robotpose_template) , Size(x_Pixe_robotpose_template + robotradius * cos(-middleangle), y_Pixe_robotpose_template + robotradius * sin(-middleangle)), Scalar(64, 255, 0), 1, LINE_AA);

        }


        if(isshowImageWithRobotangle && isshowImage){
            double distance = sqrt(pow(x_Pixe_robotpose_template-laser_Pixe_right_x,2)+pow(y_Pixe_robotpose_template-laser_Pixe_right_y,2));

        }

        cv::imshow("dsttest", dsttest);

        int newWidth = int(dsttest.cols*4);
        int newHeight = int(dsttest.rows*4);
        cv::Mat dsttest_out;
        resize(dsttest,dsttest_out, Size(newWidth, newHeight));
        cv::imshow("dsttest_out", dsttest_out);

        Mat dst2_out;
        bitwise_not(dsttest_out,dst2_out);
        cv::imshow("dst2_out", dst2_out);

    }


    Mat dst2;
    bitwise_not(dst1,dst2);

    if(isshowImage)
        cv::imshow("img_result3", dst2);

    Mat image2 = imread(image2path.c_str());


    string laser_leftfig_str="-1";
    string laser_rightfig_str="-1";
    string laser_middlefig_str="-1";


    contnav_msgs::ScanToPointToPng  scanToPointToPngmsg;
    scanToPointToPngmsg.x_Pixe_robotpose_template=to_string(x_Pixe_robotpose_template);
    scanToPointToPngmsg.y_Pixe_robotpose_template=to_string(y_Pixe_robotpose_template);
    scanToPointToPngmsg.robot_posex=to_string(pose_base_link.position.x);
    scanToPointToPngmsg.robot_posey=to_string(pose_base_link.position.y);
    if(laser_leftfig){
        laser_leftfig_str="1";
        scanToPointToPngmsg.laser_leftfig=laser_leftfig_str;
        scanToPointToPngmsg.leftangle=to_string(leftangle);
    }else{
        scanToPointToPngmsg.laser_leftfig=laser_leftfig_str;
        scanToPointToPngmsg.leftangle="";
    }

    if(laser_rightfig){
        laser_rightfig_str="1";
        scanToPointToPngmsg.laser_rightfig=laser_rightfig_str;
        scanToPointToPngmsg.rightangle=to_string(rightangle);
    }else{
        scanToPointToPngmsg.laser_rightfig=laser_rightfig_str;
        scanToPointToPngmsg.rightangle="";
    }

    if(laser_middlefig){
        laser_middlefig_str=1;
        scanToPointToPngmsg.laser_middlefig=laser_middlefig_str;

    }else{
        scanToPointToPngmsg.laser_middlefig=laser_middlefig_str;
    }

    scanToPointToPngmsg.middleangle=to_string(middleangle);
    scanToPointToPngmsg.robot_poseyaw=to_string(yaw);
    dst2_map_msgpublisher.publish(scanToPointToPngmsg);

//---------------------------------

    cv_bridge::CvImage newImage;
    newImage.header.seq = ++seq;
    newImage.header.stamp = ros::Time::now();
    newImage.image = dst2;
    newImage.encoding = "mono8";// mono8 bgr8
    const sensor_msgs::ImagePtr &image_msg = newImage.toImageMsg();
    cvBtidgepublisher.publish(image_msg);

    //--------------------------------------
    if(issave){
        string  imagepath_name = imagesavepath+"/"+to_string(ros::Time::now().sec)+".jpg";
        imwrite(imagepath_name,dst2);
        if (out.is_open())
        {
            out << imagepath_name << ",";
            out << imagepath_name << ",";
            out << to_string(x_Pixe_robotpose_template) << ",";
            out << to_string(y_Pixe_robotpose_template) << ",";
            out << to_string(pose_base_link.position.x) << ",";
            out << to_string(pose_base_link.position.y) << ",";
            out << to_string(yaw) << ",";
            out << to_string(middleangle) <<std::endl;

        }

    }

    if(startinitialpose!=-1){

        double roll,pitch,yaw;
        Matrix4d matrix4d_Transform;
        geometry_msgs::Pose pose_mapTbase_link;
        bool getrobotbaselinkfig= false;
        getrobotbaselinkfig =  getrobotbaselink("/map", "/base_link",roll,pitch,yaw ,pose_mapTbase_link,matrix4d_Transform);

        if(!getrobotbaselinkfig){
            m_.unlock();
            cout << "未获取到robotpose的位置！"  << endl;
            return;
        }

        matchTemplatesFunNew(dst2, image2,x_Pixe_robotpose_template,y_Pixe_robotpose_template,pose_mapTbase_link,yaw,dynamic_turn_angle);

    }

    waitKey(10);
    m_.unlock();

}


void ScanToPointCloud2Converter::matchTemplatesFunNew( Mat &dst2,  Mat &image2,const double &x_Pixe_robotpose_template,const double &y_Pixe_robotpose_template,const geometry_msgs::Pose  &pose_mapTbase_link,double &yaw,const double &dynamic_turn_angle) {//todo 模板匹配


    try {

       // Mat dst5 = dst2.clone();;
       // //cvtColor(dst2, dst5, COLOR_BGR2RGB);
       //  cvtColor(image2, image2, COLOR_BGR2GRAY);


        Mat dst5 = dst2.clone();;
        // cvtColor(dst2, dst5, COLOR_BGR2RGB);
        cvtColor(image2, image2, COLOR_BGR2GRAY);

        
        Mat result;
        matchTemplate(image2, dst5, result,
                      TM_CCOEFF_NORMED);

        double minVal, maxVal;
        Point minLoc, maxLoc;
        minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc);

        //dsttest.copyTo(im);

        if(isshowImage){
            Rect rect1(maxLoc.x, maxLoc.y, dst5.size().width,
                       dst5.size().height);

            cv::Mat dsttest;

            dsttest=  image2.clone();

            rectangle(dsttest, rect1, Scalar(0, 0, 255), 2);
            imshow("dst", dsttest);

        }



        if(startinitialpose!=6){

            if(maxVal>=fazhimax  ){

                cout << "满足最大值直间重定位 fazhimax"  <<endl;
                double x_Pixe_new = x_Pixe_robotpose_template + maxLoc.x;
                double y_Pixe_new = y_Pixe_robotpose_template + maxLoc.y;

                double rosX_new;
                double rosY_new;
                getglobalToRos(x_Pixe_new,y_Pixe_new,rosX_new,rosY_new);

                geometry_msgs::PoseWithCovarianceStamped pose_msg;
                pose_msg.header.stamp = ros::Time::now();
                pose_msg.header.frame_id = "map";
                pose_msg.pose.pose.position.x = rosX_new;
                pose_msg.pose.pose.position.y = rosY_new;
                pose_msg.pose.covariance[0] = 0.25;
                pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
                pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
                if(startinitialpose==3){
                    geometry_msgs::Quaternion goal_quat2 = tf::createQuaternionMsgFromYaw(0.0);

                    pose_msg.pose.pose.orientation.z = goal_quat2.z;
                    pose_msg.pose.pose.orientation.w = goal_quat2.w;
                }else{
                    pose_msg.pose.pose.orientation.z = pose_mapTbase_link.orientation.z;
                    pose_msg.pose.pose.orientation.w = pose_mapTbase_link.orientation.w;
                }


                initial_pose_pub.publish(pose_msg);
                startinitialpose=-1;
                matchTemplatestructs_.clear();
                rotatAngleStatic = rotatAngleStatic_;
                rotatAngleStaticThreadnum=0;
                timer3_.stop();
                for (int j = 0; j < 20; ++j) {
                    geometry_msgs::Twist  twist;
                    twist.angular.z=0.0;
                    cmdpub.publish(twist);
                    std::this_thread::sleep_for(std::chrono::milliseconds(50));
                }

                string messages="满足最大值直间重定位 fazhimax: "+to_string(fazhimax);
                contnav_debug(messages.c_str());//日志

                string messages1="initial_pose  rosX_new: "+to_string(rosX_new) +" ,rosY_new: "+to_string(rosY_new)+" ,qz: "+to_string(pose_msg.pose.pose.orientation.z)+" ,qw: "+to_string(pose_msg.pose.pose.orientation.w) ;
                contnav_debug(messages1.c_str());//日志

                string messages2="initial_pose  最大匹配分数maxVal: "+to_string(maxVal) +" ,x_Pixe_new: "+to_string(x_Pixe_new)+" ,y_Pixe_new: "+to_string(y_Pixe_new)+" ,x_Pixe_robotpose_template: "+to_string(x_Pixe_robotpose_template)+" ,y_Pixe_robotpose_template: "+to_string(y_Pixe_robotpose_template)+" ,重定位 fazhimin: " + to_string(fazhimin);
                contnav_debug(messages2.c_str());//日志

                return;
            }



            MatchTemplatestruct matchTemplatestruct;
            matchTemplatestruct.minVal = minVal;
            matchTemplatestruct.maxVal = maxVal;
            matchTemplatestruct.minLoc = minLoc;
            matchTemplatestruct.maxLoc = maxLoc;
            matchTemplatestruct.dst3w = dst5.size().width;
            matchTemplatestruct.dst3h = dst5.size().height;
            matchTemplatestruct.qz=pose_mapTbase_link.orientation.z;
            matchTemplatestruct.qw=pose_mapTbase_link.orientation.w;
            matchTemplatestruct.yaw=dynamic_turn_angle;
            matchTemplatestruct.x_Pixe_robotpose_template=x_Pixe_robotpose_template;
            matchTemplatestruct.y_Pixe_robotpose_template=y_Pixe_robotpose_template;
            matchTemplatestructs_.push_back(matchTemplatestruct);

            rotatAngleStaticThreadnum++;




            //---------------------------
        }else{

            if(maxVal>=fazhiLocation  ) {
                double x_Pixe_new = x_Pixe_robotpose_template + maxLoc.x;
                double y_Pixe_new = y_Pixe_robotpose_template + maxLoc.y;

                double rosX_new;
                double rosY_new;
                getglobalToRos(x_Pixe_new,y_Pixe_new,rosX_new,rosY_new);


                geometry_msgs::PoseStamped poseStamped;
                poseStamped.header.stamp=ros::Time::now();
                poseStamped.header.frame_id = "map";
                poseStamped.pose.position.x=rosX_new;
                poseStamped.pose.position.y=rosY_new;
                poseStamped.pose.position.z=0.0;
                poseStamped.pose.orientation.x=pose_mapTbase_link.orientation.x;
                poseStamped.pose.orientation.y=pose_mapTbase_link.orientation.y;
                poseStamped.pose.orientation.z=pose_mapTbase_link.orientation.z;
                poseStamped.pose.orientation.w=pose_mapTbase_link.orientation.w;
                scanToPointToPng_poseStamped_pub.publish(poseStamped);


                if(isLocationinitialPosefig){

                    geometry_msgs::PoseWithCovarianceStamped pose_msg;
                    pose_msg.header.stamp = ros::Time::now();
                    pose_msg.header.frame_id = "map";
                    pose_msg.pose.pose.position.x = rosX_new;
                    pose_msg.pose.pose.position.y = rosY_new;
                    pose_msg.pose.covariance[0] = 0.25;
                    pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
                    pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
                    pose_msg.pose.pose.orientation.z = pose_mapTbase_link.orientation.z;
                    pose_msg.pose.pose.orientation.w = pose_mapTbase_link.orientation.w;
                    initial_pose_pub.publish(pose_msg);
                }


            }//=================maxVal>=fazhiLocation==================================




        }

      //  m_.unlock();
    }catch (exception e){
       // m_.unlock();
        cout << "异常 matchTemplatesFunNew ！"  << endl;
        string messages="matchTemplatesFunNew ";
        contnav_error(messages.c_str());//日志
    }




}



void ScanToPointCloud2Converter::ScanCallbackThread(const double &rotatAngleThread)
{
           m_.lock();

       if(startinitialpose==-1){
           m_.unlock();
           return;
       }

        boost::shared_ptr<sensor_msgs::LaserScan  const>   scan_msg = ros::topic::waitForMessage<sensor_msgs::LaserScan>("/scan", ros::Duration(5));

        if (scan_msg)//scan_msg
        {

            try {

                    double roll,pitch,yaw;
                    Matrix4d matrix4d_Transform;
                    geometry_msgs::Pose pose_mapTbase_link;
                    bool getrobotbaselinkfig= false;
                    for (int j = 0; j < 10; ++j) {
                        getrobotbaselinkfig =  getrobotbaselink("/map", "/base_link",roll,pitch,yaw ,pose_mapTbase_link,matrix4d_Transform);

                        if(!getrobotbaselinkfig)
                            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                    }
                    if(!getrobotbaselinkfig){
                       m_.unlock();
                        cout << "未获取到robotpose的位置！"  << endl;
                        return;
                    }
                    double rotatRadian= rotatAngleThread * CV_PI / 180;
                    //转4元素
                    geometry_msgs::Quaternion goal_quat1= tf::createQuaternionMsgFromYaw(rotatRadian);

                    geometry_msgs::PoseWithCovarianceStamped pose_msg;
                    pose_msg.header.stamp = ros::Time::now();
                    pose_msg.header.frame_id = "map";
                    pose_msg.pose.pose.position.x = pose_mapTbase_link.position.x;
                    pose_msg.pose.pose.position.y = pose_mapTbase_link.position.y;
                    pose_msg.pose.covariance[0] = 0.25;
                    pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
                    pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
                    pose_msg.pose.pose.orientation.z = goal_quat1.z;
                    pose_msg.pose.pose.orientation.w = goal_quat1.w;//0.9999987040618878
                    initial_pose_pub.publish(pose_msg);

                //=============================================================


                sensor_msgs::LaserScan laserScanMsg = *scan_msg;
                for (int j = 0; j < laserScanMsg.ranges.size(); ++j) {
                    // 260   450
                    if(j<(laser_index_min_) || j>(laser_index_max_)){
                        laserScanMsg.ranges[j]=0;
                    }

                }


                try
                {

                    listener.lookupTransform("/map", laserFrameId, ros::Time(0), transform);  // laser  base_scan
                }
                catch(const std::exception& e)
                {
                   m_.unlock();
                    std::cerr << e.what() << '\n';
                    ROS_INFO("--/map- /base_scan -waiting--tf---\n\t");

                    return;
                }

                //==============================================



                double x,y,z;
                double rx, ry, rz;
                // 转欧拉角
                const tf::Quaternion &quaternion = transform.getRotation();

                tf::Matrix3x3(quaternion).getRPY(rx, ry, rz);

                //位置
                x=transform.getOrigin().getX(),
                        y=transform.getOrigin().getY();
                z=transform.getOrigin().getZ();

                Eigen::Vector3d laser2map_Transform_pos(x, y, z);
                Eigen::Vector3d laser2map_Transform_rot(rx, ry, rz);
                Matrix4d laser2map_Transform =Compose(laser2map_Transform_pos, laser2map_Transform_rot);


                cloud_msg->points.resize(laserScanMsg.ranges.size());
                // /**
                for (unsigned int i = 0; i < laserScanMsg.ranges.size(); ++i)
                {
                    PointT &point_tmp = cloud_msg->points[i];
                    float range = laserScanMsg.ranges[i];
                    if (!std::isfinite(range))
                    {
                        point_tmp = invalid_point_;
                        continue;
                    }

                    if (range > laserScanMsg.range_min && range < laserScanMsg.range_max)
                    {
                        double angle = laserScanMsg.angle_min + i * laserScanMsg.angle_increment;

                        double laser_x = range * cos(angle);
                        double laser_y = range * sin(angle);
                        double laser_z = 0.0;

                        Eigen::Vector3d laser_current_pos(laser_x, laser_y, laser_z);
                        Eigen::Vector3d laser_current_rot(0.0, 0.0, angle);
                        Eigen::Matrix4d laser_current =Compose(laser_current_pos, laser_current_rot);
                        Matrix4d laser2map = laser2map_Transform*laser_current;
                        VectorXd  vectorPose= H2Euler(laser2map);
                        point_tmp.x =vectorPose(0, 0);
                        point_tmp.y = vectorPose(1, 0);


                    }
                    else{
                        point_tmp = invalid_point_;

                    }

                }
                use_pcdTopicture_main(rotatAngleThread);

            } catch(const std::exception& e)
            {

               m_.unlock();
                std::cerr << e.what() << '\n';
                ROS_INFO("----waiting-----\n\t");
            }




//===============================================================================

        }else{//end scan
         //未获取到数据
            cout << "=================未获取到scan数据 ======================"<<endl;

         }


   // m_.unlock();


}
void ScanToPointCloud2Converter::ScanCallbackDynamicThread(const double &dynamic_turn_angle)
{
             m_.lock();


    if(startinitialpose==-1){
        m_.unlock();
        return;
    }

    boost::shared_ptr<sensor_msgs::LaserScan  const>   scan_msg = ros::topic::waitForMessage<sensor_msgs::LaserScan>("/scan", ros::Duration(5));

    if (scan_msg)//scan_msg
    {
        try {


            sensor_msgs::LaserScan laserScanMsg = *scan_msg;
            for (int j = 0; j < laserScanMsg.ranges.size(); ++j) {
                // 260   450
                if(j<(laser_index_min_) || j>(laser_index_max_)){
                    laserScanMsg.ranges[j]=0;
                }

            }

            try
            {

                 listener.lookupTransform("/base_link", laserFrameId, ros::Time(0), transform);  // laser  base_scan
            }
            catch(const std::exception& e)
            {
                m_.unlock();
                std::cerr << e.what() << '\n';
                ROS_INFO("--/map- /base_scan -waiting--tf---\n\t");

                return;
            }


            double x,y,z;
            double rx, ry, rz;//定义存储
            // 转欧拉角
            const tf::Quaternion &quaternion = transform.getRotation();

            tf::Matrix3x3(quaternion).getRPY(rx, ry, rz);//

            //位置
            x=transform.getOrigin().getX(),
                    y=transform.getOrigin().getY();
            z=transform.getOrigin().getZ();

            Eigen::Vector3d laser2map_Transform_pos(x, y, z);
            Eigen::Vector3d laser2map_Transform_rot(rx, ry, rz);
            Matrix4d laser2map_Transform =Compose(laser2map_Transform_pos, laser2map_Transform_rot);



            cloud_msg->points.resize(laserScanMsg.ranges.size());
            // /**
            for (unsigned int i = 0; i < laserScanMsg.ranges.size(); ++i)
            {
                PointT &point_tmp = cloud_msg->points[i];
                float range = laserScanMsg.ranges[i];
                if (!std::isfinite(range))
                {
                    point_tmp = invalid_point_;
                    continue;
                }

                if (range > laserScanMsg.range_min && range < laserScanMsg.range_max)
                {
                    double angle = laserScanMsg.angle_min + i * laserScanMsg.angle_increment;

                    double laser_x = range * cos(angle);
                    double laser_y = range * sin(angle);
                    double laser_z = 0.0;


                    Eigen::Vector3d laser_current_pos(laser_x, laser_y, laser_z);
                    Eigen::Vector3d laser_current_rot(0.0, 0.0, angle);
                    Eigen::Matrix4d laser_current =Compose(laser_current_pos, laser_current_rot);
                    Matrix4d laser2map = laser2map_Transform*laser_current;
                    VectorXd  vectorPose= H2Euler(laser2map);
                    point_tmp.x =vectorPose(0, 0);
                    point_tmp.y = vectorPose(1, 0);


                }
                else{

                    point_tmp = invalid_point_;

                }

            }
            use_pcdTopicture_main(dynamic_turn_angle);


        } catch(const std::exception& e)
        {

            m_.unlock();
            std::cerr << e.what() << '\n';
            ROS_INFO("----waiting-----\n\t");
        }




//===============================================================================

    }else{//end scan
        //未获取到数据
        cout << "=================未获取到scan数据 ======================"<<endl;

    }


    // m_.unlock();


}
void ScanToPointCloud2Converter::ScanCallbacklocationThread(const double &rotatAngleThread)
{
    while (startinitialpose ==6){
        ScanCallbacklocation(rotatAngleThread);

    }
}
void ScanToPointCloud2Converter::ScanCallbacklocation(const double &rotatAngleThread)
{

    bool tryLock = m_.try_lock();
    if(!tryLock){

        ROS_INFO("ScanCallback  bool tryLock = m_.try_lock(); --\n\t");
        return;

    }


    if(startinitialpose==-1){
        m_.unlock();
        return;
    }

    boost::shared_ptr<sensor_msgs::LaserScan  const>   scan_msg = ros::topic::waitForMessage<sensor_msgs::LaserScan>("/scan", ros::Duration(5));

    if (scan_msg)//scan_msg
    {

        try {


            sensor_msgs::LaserScan laserScanMsg = *scan_msg;
            //  260   450   ||  360
            for (int j = 0; j < laserScanMsg.ranges.size(); ++j) {
                // 260   450
                if(j<(laser_index_min_) || j>(laser_index_max_)){
                    laserScanMsg.ranges[j]=0;
                }

            }


            try
            {

                listener.lookupTransform("/map", laserFrameId, ros::Time(0), transform);  // laser  base_scan
            }
            catch(const std::exception& e)
            {
                m_.unlock();
                std::cerr << e.what() << '\n';
                ROS_INFO("--/map- /base_scan -waiting--tf---\n\t");

                return;
            }




            double x,y,z;
            double rx, ry, rz;//定义存储r\p\y的容器

            const tf::Quaternion &quaternion = transform.getRotation();

            tf::Matrix3x3(quaternion).getRPY(rx, ry, rz);//

            x=transform.getOrigin().getX(),
                    y=transform.getOrigin().getY();
            z=transform.getOrigin().getZ();


            Eigen::Vector3d laser2map_Transform_pos(x, y, z);
            Eigen::Vector3d laser2map_Transform_rot(rx, ry, rz);
            Matrix4d laser2map_Transform =Compose(laser2map_Transform_pos, laser2map_Transform_rot);//



            cloud_msg->points.resize(laserScanMsg.ranges.size());
            // /**
            for (unsigned int i = 0; i < laserScanMsg.ranges.size(); ++i)
            {
                PointT &point_tmp = cloud_msg->points[i];
                float range = laserScanMsg.ranges[i];
                if (!std::isfinite(range))
                {
                    point_tmp = invalid_point_;
                    continue;
                }

                if (range > laserScanMsg.range_min && range < laserScanMsg.range_max)
                {
                    double angle = laserScanMsg.angle_min + i * laserScanMsg.angle_increment;

                    double laser_x = range * cos(angle);
                    double laser_y = range * sin(angle);
                    double laser_z = 0.0;

                    Eigen::Vector3d laser_current_pos(laser_x, laser_y, laser_z);
                    Eigen::Vector3d laser_current_rot(0.0, 0.0, angle);
                    Eigen::Matrix4d laser_current =Compose(laser_current_pos, laser_current_rot);
                    Matrix4d laser2map = laser2map_Transform*laser_current;
                    VectorXd  vectorPose= H2Euler(laser2map);
                    point_tmp.x =vectorPose(0, 0);
                    point_tmp.y = vectorPose(1, 0);


                }
                else{

                    point_tmp = invalid_point_;

                }

            }
            use_pcdTopicture_main(rotatAngleThread);



        } catch(const std::exception& e)
        {

            m_.unlock();
            std::cerr << e.what() << '\n';
            ROS_INFO("----waiting-----\n\t");
        }






    }else{//end scan
        //未获取到数据
        cout << "=================未获取到scan数据 ======================"<<endl;

    }



}

void ScanToPointCloud2Converter::scanCallbackThreadNew()
{


    if(startinitialpose==-1){
        m_new_.unlock();
        return;
    }




    double roll,pitch,yaw;
    Matrix4d mapTbase_link_matrix4d_Transform;
    geometry_msgs::Pose pose_mapTbase_link;
    bool getrobotbaselinkfig= false;
    for (int j = 0; j < 10; ++j) {
        getrobotbaselinkfig =  getrobotbaselink("/map", "/base_link",roll,pitch,yaw ,pose_mapTbase_link,mapTbase_link_matrix4d_Transform);

        if(!getrobotbaselinkfig)
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    if(!getrobotbaselinkfig){
        m_new_.unlock();
        cout << "未获取到robotpose的位置！"  << endl;

        return;
    }



    tf::StampedTransform transformbaselinkTlaser;
    try
    {


        listener.lookupTransform("/base_link", laserFrameId, ros::Time(0), transformbaselinkTlaser);  // laser  base_scan
    }
    catch(const std::exception& e)
    {
        m_new_.unlock();
        std::cerr << e.what() << '\n';
        ROS_INFO("--/base_link- /base_scan -waiting--tf---\n\t");
        return;
    }

    double baselinkTlaser_x,baselinkTlaser_y,baselinkTlaser_z;
    double baselinkTlaser_rx, baselinkTlaser_ry, baselinkTlaser_rz;
    // 4元数转欧拉角
    const tf::Quaternion &baselinkTlaserquaternion = transformbaselinkTlaser.getRotation();

    tf::Matrix3x3(baselinkTlaserquaternion).getRPY(baselinkTlaser_rx, baselinkTlaser_ry, baselinkTlaser_rz);


    baselinkTlaser_x=transformbaselinkTlaser.getOrigin().getX(),
            baselinkTlaser_y=transformbaselinkTlaser.getOrigin().getY();
    baselinkTlaser_z=transformbaselinkTlaser.getOrigin().getZ();


    Eigen::Vector3d baselinkTlaser_Transform_pos(baselinkTlaser_x, baselinkTlaser_y, baselinkTlaser_z);
    Eigen::Vector3d baselinkTlaser_Transform_rot(baselinkTlaser_rx, baselinkTlaser_ry, baselinkTlaser_rz);
    Matrix4d baselinkTlaser_Transform =Compose(baselinkTlaser_Transform_pos, baselinkTlaser_Transform_rot);


    boost::shared_ptr<sensor_msgs::LaserScan  const>   scan_msg = ros::topic::waitForMessage<sensor_msgs::LaserScan>("/scan", ros::Duration(5));

    if (scan_msg)
    {

        try {

            Mat image2 = imread(image2path.c_str());
            sensor_msgs::LaserScan laserScanMsg = *scan_msg;

            for (int j = 0; j < laserScanMsg.ranges.size(); ++j) {

                if(j<(laser_index_min_) || j>(laser_index_max_)){
                    laserScanMsg.ranges[j]=0;
                }

            }



            while (rotatAngleStatic>0 && startinitialpose!=-1){

                double rotatRadian= rotatAngleStatic * CV_PI / 180;

                if(isshowImage){
                    std::this_thread::sleep_for(std::chrono::milliseconds(20));
                }else{
                    std::this_thread::sleep_for(std::chrono::milliseconds(5));
                }
                boost::thread ScanCallbackThreadNew_thread(boost::bind(&ScanToPointCloud2Converter::scantoimageInitposeNew, this,pose_mapTbase_link, baselinkTlaser_Transform,
                                                                       image2, laserScanMsg,
                                                                       rotatRadian));
                rotatAngleStatic=rotatAngleStatic-angleStaticxishu;


            }//while (rotatAngleStatic>0 && startinitialpose!=-1){



        } catch(const std::exception& e)
        {

            m_.unlock();
            m_scanToPointToPngrFun_.unlock();
            std::cerr << e.what() << '\n';
            ROS_INFO("----waiting-----\n\t");
        }






    }else{//end scan

        cout << "=================未获取到scan数据 ======================"<<endl;

    }


    // m_.unlock();


}
void ScanToPointCloud2Converter::scantoimageInitposeNew(
        geometry_msgs::Pose pose_mapTbase_link,
        const Matrix4d &baselinkTlaser_Transform,  Mat &image2,
        const sensor_msgs::LaserScan &laserScanMsg,
        double rotatRadian) {


    pcl::PointCloud<pcl::PointXYZ> cloud_msgnew;
    cloud_msgnew.resize(laserScanMsg.ranges.size());


    geometry_msgs::Quaternion goal_quat1 = tf::createQuaternionMsgFromYaw(rotatRadian);

    Vector3d mapTbase_link_Transform_pos(pose_mapTbase_link.position.x, pose_mapTbase_link.position.y,
                                         pose_mapTbase_link.position.z);
    Vector3d mapTbase_link_Transform_rot(0.0, 0.0, rotatRadian);
    Matrix4d mapTbase_link_matrix4d_Transform = Compose(mapTbase_link_Transform_pos,
                                                        mapTbase_link_Transform_rot);


    pose_mapTbase_link.orientation.z=     goal_quat1.z;
    pose_mapTbase_link.orientation.w=     goal_quat1.w;


    Matrix4d mapTlaser_Transform  = mapTbase_link_matrix4d_Transform*baselinkTlaser_Transform;


    for (unsigned int i = 0; i < laserScanMsg.ranges.size(); ++i) {

        pcl::PointXYZ &point_tmp = cloud_msgnew.points[i];

        float range = laserScanMsg.ranges[i];

        if (!isfinite(range)) {

            cloud_msgnew.points[i] = invalid_point_;
            continue;
        }


        if (range > laserScanMsg.range_min && range < laserScanMsg.range_max) {

            double angle = laserScanMsg.angle_min + i * laserScanMsg.angle_increment;

            double laser_x = range * cos(angle);
            double laser_y = range * sin(angle);
            double laser_z = 0.0;


            Vector3d laser_current_pos(laser_x, laser_y, laser_z);
            Vector3d laser_current_rot(0.0, 0.0, angle);

            Matrix4d laser_current = Compose(laser_current_pos, laser_current_rot);
            Matrix4d laser2map = mapTlaser_Transform * laser_current;

            VectorXd vectorPose = H2Euler(laser2map);
            point_tmp.x = vectorPose(0, 0);
            point_tmp.y = vectorPose(1, 0);


        } else {
            point_tmp = invalid_point_;

        }

    }


    pcl::PointXYZ min, max;

    pcl::getMinMax3D(cloud_msgnew, min, max);


    vector<int> indices;
    cout << "2cloud_msg.points.size(): " << cloud_msgnew.points.size() << endl;
    for (int i = 0; i < cloud_msgnew.points.size(); i++) {

        indices.push_back(i);

    };

    vector<int> x_img;
    vector<int> y_img;

    int x_max = 0;
    int y_max = 0;
    int x_min = 10000;
    int y_min = 10000;
    double scene_x = 0 - (origin_x / resolution_f);
    double scene_y = height + (origin_y / resolution_f);


    cout << "indices.size(): " << indices.size() << endl;
    for (int i = 0; i < indices.size(); i++) {
        if ((abs(cloud_msgnew.points[i].x) + abs(cloud_msgnew.points[i].y)) < 0.000001)
            continue;


        double rosX = cloud_msgnew.points[i].x;
        double rosY = cloud_msgnew.points[i].y;

        double x_Pixe_new;
        double y_Pixe_new;
        rosToGlobal_one(rosX, rosY, resolution_f, resolution_f, scene_x, scene_y, x_Pixe_new, y_Pixe_new);
        x_img.push_back(x_Pixe_new < 0 ? 0 : x_Pixe_new);
        y_img.push_back(y_Pixe_new < 0 ? 0 : y_Pixe_new);

        if (x_Pixe_new > x_max)
            x_max = x_Pixe_new;
        if (y_Pixe_new > y_max)
            y_max = y_Pixe_new;

        if (x_Pixe_new < x_min)
            x_min = x_Pixe_new < 0 ? 0 : x_Pixe_new;
        if (y_Pixe_new < y_min)
            y_min = y_Pixe_new < 0 ? 0 : y_Pixe_new;

    }
    double x_Pixe_robotpose;
    double y_Pixe_robotpose;


    getrosToGlobal(pose_mapTbase_link.position.x, pose_mapTbase_link.position.y, x_Pixe_robotpose, y_Pixe_robotpose);

    if (showrobotposefig&&isshowImage) {
        x_img.push_back(x_Pixe_robotpose);
        y_img.push_back(y_Pixe_robotpose);

        if (x_Pixe_robotpose > x_max)
            x_max = x_Pixe_robotpose;
        if (y_Pixe_robotpose > y_max)
            y_max = y_Pixe_robotpose;
        //获取宽和高,获取最大值
        if (x_Pixe_robotpose < x_min)
            x_min = x_Pixe_robotpose;
        if (y_Pixe_robotpose < y_min)
            y_min = y_Pixe_robotpose;
    }



    y_min = y_min > 10 ? y_min - 10 : y_min;
    x_min = x_min > 10 ? x_min - 10 : x_min;


    Mat im = Mat::zeros(y_max - y_min + 10, x_max - x_min + 10, CV_8U);

    for (int i = 0; i < x_img.size(); i++) {
        im.at<uchar>((y_img[i] - y_min), (x_img[i] - x_min)) = 255;

    }


    double x_Pixe_robotpose_template = x_Pixe_robotpose - x_min;
    double y_Pixe_robotpose_template = y_Pixe_robotpose - y_min;

    if (showrobotposefig&&isshowImage) {
        cout << "x_Pixe_robotpose_template: " << x_Pixe_robotpose_template << endl;
        cout << "y_Pixe_robotpose_template: " << y_Pixe_robotpose_template << endl;

        im.at<uchar>(y_Pixe_robotpose_template, x_Pixe_robotpose_template) = 150;
    }



    if (isshowImage)
        imshow("img_result", im);




    Mat dst;

    Mat kernel = getStructuringElement(MORPH_RECT, Size(10, 10));
    dilate(im, dst, kernel);

    if (isshowImage)
        imshow("img_result2", dst);

    Mat dst1;
    Mat kernel2 = getStructuringElement(MORPH_RECT, Size(8, 8));

    erode(dst, dst1, kernel2);

    Mat dst2;
    bitwise_not(dst1, dst2);



    if (isshowImage)
        imshow("img_result3", dst2);


    if (startinitialpose != -1) {

        matchTemplatesFunNew2(dst2, image2, x_Pixe_robotpose_template, y_Pixe_robotpose_template, pose_mapTbase_link);
    }

    waitKey(10);
}

void ScanToPointCloud2Converter::matchTemplatesFunNew2( Mat dst2,  Mat &image2,const double &x_Pixe_robotpose_template,const double &y_Pixe_robotpose_template,const geometry_msgs::Pose  &pose_mapTbase_link) {//todo 模板匹配


    try {


       ///// Mat dst5;
        ////cvtColor(dst2, dst5, COLOR_BGR2RGB);

       //  Mat dst5 = dst2.clone();;
       // //cvtColor(dst2, dst5, COLOR_BGR2RGB);
        // cvtColor(image2, image2, COLOR_BGR2GRAY);

            Mat dst5 = dst2.clone();;
            // cvtColor(dst2, dst5, COLOR_BGR2RGB);
            cvtColor(image2, image2, COLOR_BGR2GRAY);

        Mat result;
        matchTemplate(image2, dst5, result,
                      TM_CCOEFF_NORMED);


        double minVal, maxVal;
        Point minLoc, maxLoc;
        minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc);

        if(isshowImage){

            Rect rect1(maxLoc.x, maxLoc.y, dst5.size().width,
                       dst5.size().height);

            Mat image2clone=image2.clone();
            rectangle(image2clone, rect1, Scalar(0, 0, 255), 2);
            imshow("dst", image2clone);
        }


        if(startinitialpose!=6){



            if(maxVal>=fazhimax  ){

                double x_Pixe_new = x_Pixe_robotpose_template + maxLoc.x;
                double y_Pixe_new = y_Pixe_robotpose_template + maxLoc.y;
                double rosX_new;
                double rosY_new;
                getglobalToRos(x_Pixe_new,y_Pixe_new,rosX_new,rosY_new);
                geometry_msgs::PoseWithCovarianceStamped pose_msg;
                pose_msg.header.stamp = ros::Time::now();
                pose_msg.header.frame_id = "map";
                pose_msg.pose.pose.position.x = rosX_new;
                pose_msg.pose.pose.position.y = rosY_new;
                pose_msg.pose.covariance[0] = 0.25;
                pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
                pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;

                pose_msg.pose.pose.orientation.z = pose_mapTbase_link.orientation.z;
                pose_msg.pose.pose.orientation.w = pose_mapTbase_link.orientation.w;

                initial_pose_pub.publish(pose_msg);
                startinitialpose=-1;
                matchTemplatestructs_.clear();
                rotatAngleStatic = rotatAngleStatic_;
                rotatAngleStaticThreadnum=0;
                timer3_.stop();
                for (int j = 0; j < 20; ++j) {
                    geometry_msgs::Twist  twist;
                    twist.angular.z=0.0;
                    cmdpub.publish(twist);
                    std::this_thread::sleep_for(std::chrono::milliseconds(50));
                }

                return;
            }



            MatchTemplatestruct matchTemplatestruct;
            matchTemplatestruct.minVal = minVal;
            matchTemplatestruct.maxVal = maxVal;
            matchTemplatestruct.minLoc = minLoc;
            matchTemplatestruct.maxLoc = maxLoc;
            matchTemplatestruct.dst3w = dst5.size().width;
            matchTemplatestruct.dst3h = dst5.size().height;
            matchTemplatestruct.qz=pose_mapTbase_link.orientation.z;
            matchTemplatestruct.qw=pose_mapTbase_link.orientation.w;
            matchTemplatestruct.x_Pixe_robotpose_template=x_Pixe_robotpose_template;
            matchTemplatestruct.y_Pixe_robotpose_template=y_Pixe_robotpose_template;
            std::this_thread::sleep_for(std::chrono::milliseconds(5));

            m_new_.lock();
            matchTemplatestructs_.push_back(matchTemplatestruct);
            rotatAngleStaticThreadnum++;
            m_new_.unlock();



        }else{

            if(maxVal>=fazhiLocation  ) {

                double x_Pixe_new = x_Pixe_robotpose_template + maxLoc.x;
                double y_Pixe_new = y_Pixe_robotpose_template + maxLoc.y;

                double rosX_new;
                double rosY_new;
                getglobalToRos(x_Pixe_new,y_Pixe_new,rosX_new,rosY_new);

                //融合定位发布的坐标
                geometry_msgs::PoseStamped poseStamped;
                poseStamped.header.stamp=ros::Time::now();
                poseStamped.header.frame_id = "map";
                poseStamped.pose.position.x=rosX_new;
                poseStamped.pose.position.y=rosY_new;
                poseStamped.pose.position.z=0.0;
                poseStamped.pose.orientation.x=pose_mapTbase_link.orientation.x;
                poseStamped.pose.orientation.y=pose_mapTbase_link.orientation.y;
                poseStamped.pose.orientation.z=pose_mapTbase_link.orientation.z;
                poseStamped.pose.orientation.w=pose_mapTbase_link.orientation.w;
                scanToPointToPng_poseStamped_pub.publish(poseStamped);


                if(isLocationinitialPosefig){
                    geometry_msgs::PoseWithCovarianceStamped pose_msg;
                    pose_msg.header.stamp = ros::Time::now();
                    pose_msg.header.frame_id = "map";
                    pose_msg.pose.pose.position.x = rosX_new;
                    pose_msg.pose.pose.position.y = rosY_new;
                    pose_msg.pose.covariance[0] = 0.25;
                    pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
                    pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
                    pose_msg.pose.pose.orientation.z = pose_mapTbase_link.orientation.z;
                    pose_msg.pose.pose.orientation.w = pose_mapTbase_link.orientation.w;
                    initial_pose_pub.publish(pose_msg);
                }


            }

        }
        //  m_.unlock();
    }catch (exception e){
        m_new_.unlock();
        cout << "异常 matchTemplatesFunNew2 ！"  << endl;
        string messages="matchTemplatesFunNew2";
        contnav_error(messages.c_str());//日志
    }




}



std::vector<std::string>  ScanToPointCloud2Converter::split_data(std::string pattern,std::string str)
{

    std::vector<std::string> result;
    try {

        std::string::size_type pos;

        pattern+=str;
        int  size=pattern.size();

        for ( int  i=0; i<size; i++)
        {
            pos=pattern.find(str,i);
            if (pos<size)
            {
                std::string s=pattern.substr(i,pos-i);
                if (s.size()>0){
                    result.push_back(s);
                }
                i=pos+str.size()-1;
            }
        }


    }catch (std::exception e){
        //qDebug() << "CarStartServuceClient::split 抛出异常！";
    }
    return result;
}




//ctrl + c
void mySigintHandler(int sig)
{

    cout<<"ctrl c"<<endl;
if(scan_to_cloud_converter!=NULL)
     delete scan_to_cloud_converter;

    ros::shutdown();
}



int main(int argc, char **argv)
{
    ros::init(argc, argv, "scan_to_pointclod2_to_png_initialPose_node");
    ros::NodeHandle node_handle_;
    signal(SIGINT, mySigintHandler);


    scan_to_cloud_converter=new ScanToPointCloud2Converter();


    ros::spin();
    return 0;
}
